/*
 * Kruispunt_AVR.c
 *
 * Created: 15-5-2013 9:58:12
 *  Author: Matthijs
 */


#include <avr/io.h>
#include <avr/delay.h>

int main(void)
{	
	void init();
	
    int presentState = 1;
	int stoplichtArray[6];
	uint8_t orByteA;
	uint8_t orByteB;
	uint8_t orByteC;
	uint8_t orByteD;
	
    _delay_ms(1000);

    while(1)
    {
        int tellerEen, tellerTwee = 0, tellerDrie = 0, tellerVier;

		void Sensor_Detection();
		
        //Process
        if(presentState==1)//Gedaan
        {
            tellerVier = 1;
            int staArray[10][7] =
            {
                {0,1,0,0,1,0,2},
                {0,0,1,0,0,1,4},
                {1,0,0,0,0,0,6},
                {0,0,0,1,0,0,8},
                {0,1,1,0,0,0,10},
                {0,0,0,0,1,1,12},
                {0,1,0,0,0,0,25},
                {0,0,1,0,0,0,26},
                {0,0,0,0,1,0,27},
                {0,0,0,0,0,1,28}
            };
            for(tellerEen=0; tellerEen<10; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==2)//Gedaan
        {
            tellerVier = 3;
            int staArray[5][7] =
            {
                {0,1,0,0,0,0,14},
                {0,0,0,0,1,0,17},
                {0,1,0,0,1,0,2},
                {0,1,0,0,0,0,30},
                {0,0,0,0,1,0,35}
            };
            for(tellerEen=0; tellerEen<5; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==3)//Gedaan
        {
            tellerVier = 1;
        }
        else if(presentState==4)//Gedaan
        {
            tellerVier = 5;
            int staArray[5][7] =
            {
                {0,0,1,0,0,0,31},
                {0,0,0,0,1,1,29},
                {0,1,1,0,0,0,20},
                {0,0,1,0,0,1,4},
                {0,0,0,0,0,1,34}
            };
            for(tellerEen=0; tellerEen<5; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==5)//Gedaan
        {
            tellerVier=1;
        }
        else if(presentState==6)//Gedaan
        {
            tellerVier = 7;
            int staArray[1][7] =
            {
                {1,0,0,0,0,0,6},
            };
            for(tellerEen=0; tellerEen<1; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==7)//Gedaan
        {
            tellerVier=1;
        }
        else if(presentState==8)//Gedaan
        {
            tellerVier = 9;
            int staArray[1][7] =
            {
                {0,0,0,4,0,0,8}
            };
            for(tellerEen=0; tellerEen<1; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==9)//Gedaan
        {
            tellerVier=1;
        }
        else if(presentState==10)//Gedaan
        {
            tellerVier = 11;
            int staArray[5][7] =
            {
                {0,0,1,0,0,1,19},
                {0,1,0,0,1,0,15},
                {0,0,1,0,0,0,33},
                {0,1,0,0,0,0,36},
                {0,1,1,0,0,0,10},
            };
            for(tellerEen=0; tellerEen<5; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==11)//Gedaan
        {
            tellerVier=1;
        }
        else if(presentState==12)//Gedaan
        {
            tellerVier = 13;
            int staArray[5][7] =
            {
                {0,1,0,0,1,0,16},
                {0,0,1,0,0,1,18},
                {0,0,0,0,0,1,37},
                {0,0,0,0,1,0,32},
                {0,0,0,0,1,1,12},
            };
            for(tellerEen=0; tellerEen<5; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==13)//Gedaan
        {
            tellerVier=1;
        }
        else if(presentState==14)//Gedaan
        {
            tellerVier = 10;
        }
        else if(presentState==15)//Gedaan
        {
            tellerVier = 2;
        }
        else if(presentState==16)//Gedaan
        {
            tellerVier = 2;
        }
        else if(presentState==17)//Gedaan
        {
            tellerVier=12;
        }
        else if(presentState==18)//Gedaan
        {
            tellerVier = 4;
        }
        else if(presentState==19)//Gedaan
        {
            tellerVier = 4;
        }
        else if(presentState==20)//Gedaan
        {
            tellerVier = 10;
        }
        else if(presentState==21)//Gedaan
        {
            tellerVier = 1;
        }
        else if(presentState==22)//Gedaan
        {
            tellerVier = 1;
        }
        else if(presentState==23)//Gedaan
        {
            tellerVier = 1;
        }
        else if(presentState==24)//Gedaan
        {
            tellerVier = 1;
        }
        else if(presentState==25)//Gedaan
        {
            tellerVier = 21;
            int staArray[3][7] =
            {
                {0,1,0,0,0,0,25},
                {0,1,0,0,1,0,2},
                {0,1,1,0,0,0,10}
            };
            for(tellerEen=0; tellerEen<3; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==26)//Gedaan
        {
            tellerVier = 22;
            int staArray[3][7] =
            {
                {0,0,1,0,0,0,26},
                {0,1,1,0,0,0,10},
                {0,0,1,0,0,1,4}
            };
            for(tellerEen=0; tellerEen<3; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==27)//Gedaan
        {
            tellerVier = 23;
            int staArray[3][7] =
            {
                {0,0,0,0,1,0,27},
                {0,1,0,0,1,0,2},
                {0,0,0,0,1,1,12}
            };
            for(tellerEen=0; tellerEen<3; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==28)//Gedaan
        {
            tellerVier = 24;
            int staArray[3][7] =
            {
                {0,0,0,0,0,1,28},
                {0,0,0,0,1,1,12},
                {0,0,1,0,0,1,4}
            };
            for(tellerEen=0; tellerEen<3; tellerEen++)
            {
                tellerDrie=0;
                for(tellerTwee=0; tellerTwee<6; tellerTwee++)
                {
                    if(senArray[tellerTwee]==staArray[tellerEen][tellerTwee])
                    {
                        tellerDrie++;
                        if(tellerDrie==6)
                        {
                            tellerVier=staArray[tellerEen][6];
                        }
                    }
                }
            }
        }
        else if(presentState==29)//Gedaan
        {
            tellerVier = 12;
        }
        else if(presentState==30)//Gedaan
        {
            tellerVier = 25;
        }
        else if(presentState==31)//Gedaan
        {
            tellerVier = 26;
        }
        else if(presentState==32)//Gedaan
        {
            tellerVier = 27;
        }
        else if(presentState==33)//Gedaan
        {
            tellerVier = 26;
        }
        else if(presentState==34)//Gedaan
        {
            tellerVier = 28;
        }
        else if(presentState==35)//Gedaan
        {
            tellerVier = 27;
        }
        else if(presentState==36)//Gedaan
        {
            tellerVier = 25;
        }
        else if(presentState==37)//Gedaan
        {
            tellerVier = 28;
        }
        presentState = tellerVier;
		
		//uitvoer
		for(tellerEen=0; tellerEen<6; tellerEen++)
		{
			stoplichtArray[tellerEen] = ledArray[presentState-1][tellerEen];
		}
		
		//---------------------------------------------------------------------------------
		
		if(stoplichtArray[0] == 1)//klopt
		{
			orByteB |= 0b00000010;
		}
		else if(stoplichtArray[0] == 2)//klopt
		{
			orByteB |= 0b00000100;
		}
		else if(stoplichtArray[0] == 3)//klopt
		{
			orByteB |= 0b00001000;
		}
		//------------------------------------
		if(stoplichtArray[1] == 1)//klopt
		{
			orByteA |= 0b10000000;
		}
		else if(stoplichtArray[1] == 2)//klopt
		{
			orByteA |= 0b01000000;
		}
		else if(stoplichtArray[1] == 3)//klopt
		{
			orByteB |= 0b00000001;
		}
		//------------------------------------
		if(stoplichtArray[2] == 1)//klopt
		{
			orByteC |= 0b00010000;
		}
		else if(stoplichtArray[2] == 2)//klopt
		{
			orByteC |= 0b00001000;
		}
		else if(stoplichtArray[2] == 3)//klopt
		{
			orByteC |= 0b00000100;
		}
		//------------------------------------
		if(stoplichtArray[3] == 1)//klopt
		{
			orByteC |= 0b10000000;
		}
		else if(stoplichtArray[3] == 2)//klopt
		{
			orByteC |= 0b01000000;
		}
		else if(stoplichtArray[3] == 3)//klopt
		{
			orByteC |= 0b00100000;
		}
		//------------------------------------
		if(stoplichtArray[4] == 1)//klopt
		{
			orByteD |= 0b00100000;
		}
		else if(stoplichtArray[4] == 2)//klopt
		{
			orByteD |= 0b01000000;
		}
		else if(stoplichtArray[4] == 3)//klopt
		{
			orByteD |= 0b10000000;
		}
		//------------------------------------
		if(stoplichtArray[5] == 1)//klopt
		{
			orByteD |= 0b00000100;
		}
		else if(stoplichtArray[5] == 2)//klopt
		{
			orByteD |= 0b00001000;
		}
		else if(stoplichtArray[5] == 3)//klopt
		{
			orByteD |= 0b00010000;
		}		
		
		PORTA = orByteA;
		PORTB = orByteB;
		PORTC = orByteC;
		PORTD = orByteD;
		
		if(orByteB == 0b00001000 || orByteB == 0b00000011 || orByteC == 0b10000100 || orByteC == 0b00110000 || orByteD == 0b10000100 || orByteD == 0b00010100 || (orByteB == 0b00000011 && orByteD == 0b10000100) || (orByteC == 0b00000100 && orByteD == 0b00100100) || (orByteB == 0b00000010 && orByteA == 0b10000000) || orByteD == 0b00100100)
		{
			_delay_ms(1000);
		}
		else if(orByteB == 0b00000100 || orByteA == 0b10000000 || orByteC == 0b10001000 || orByteC == 0b01010000 || orByteD == 0b01000100 || orByteD == 0b01001000 )
		{
			_delay_ms(500);
		}
		else
		{
			_delay_ms(100);
		}
		
		orByteA = 0b00000000;
		orByteB = 0b00000000;
		orByteC = 0b00000000;
		orByteD = 0b00000000;							
    }
}

void init()
{
	DDRC = (1<<PC7) | (1<<PC6) | (1<<PC5) | (1<<PC4)
	| (1<<PC3) | (1<<PC2) | (1<<PC1) | (1<<PC0);

	PORTC &= ~((1<<PC7) | (1<<PC6) | (1<<PC5) | (1<<PC4)
	| (1<<PC3) | (1<<PC2) | (1<<PC1) | (1<<PC0));
	
	DDRB = 0x0F;
	DDRA = 0xC0;
	DDRD = 0xFC;
}


void Sensor_Detection()
{
	int senArray[6];

//Invoer sensoren kruispunt.
	{
		if (PINA & (1<<0))
		{
			senArray[0] = 1;
		}
		else
		{
			senArray[0]= 0;
		}

		if (PINA & (1<<1))
		{
			senArray[1] = 1;
		}
		else
		{
			senArray[1]=0;
		}

		if (PINA & (1<<2))
		{
			senArray[2] = 1;
		}
		else
		{
			senArray[2]=0;
		}
		if (PINA & (1<<3))
		{
			senArray[3] = 1;
		}
		else
		{
			senArray[3] = 0;
		}

		if (PINA & (1<<4))
		{
			senArray[4] = 1;
		}
		else
		{
			senArray[4] = 0;
		}

		if (PINA & (1<<5))
		{
			senArray[5] = 1;
		}
		else
		{
			senArray[5] = 0;
		}
	}
}